package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S2: minimizes bends, i.e. changes in the curvature.
 */
public class ForceS2 extends ForceSubsystem {

	ForceS2(ForceSystem system) {
		super(system);
	}

	void applyForces(int delta) {
		for (MetroLine l : system.metroMap.lines) {
			// Convert station array to Vec2 array
			Vec2[] samples = new Vec2[l.stations.length];
			for (int i = 0; i < l.stations.length; i++) {
				Body b = system.stationBodies[l.stations[i].id];
				Vec2 p = b.getPosition();
				samples[i] = p;
			}

			if (samples.length >= 4) {
				int numCtrlPoint = Math.max(4, samples.length / 6);
				Vec2[] spline = MetroLineToBSpline.fit(samples, 2, numCtrlPoint);
				for (int i = 0; i < l.stations.length; i++) {
					MetroStation m = l.stations[i];
					m.bsX = spline[i].x;
					m.bsY = spline[i].y;

					Body b = system.stationBodies[m.id];

					Vec2 p = b.getPosition();
					Vec2 f = spline[i].sub(p);
					float len = f.normalize();

					if (len > ForceConstants.S2_ATTRACT2) {
						double fmag = ForceConstants.S2_ATTRACT1 * Math.log(len / ForceConstants.S2_ATTRACT2);

						f.mulLocal((float) fmag);

						b.applyForceToCenter(f);
					}
				}
			}
		}
	}
}
